function [RealPath,maps_type]=AStar3(obstacle,map)

% 所有的H值
HList = Calculate_HList(map,obstacle);

% 未知障碍物
% unknown_obstacle=obstacle;

% 探测范围
detective_area=50;

% 储存路径
path=[];

% 所有区域父节点
father_list=zeros(map.X,map.Y,2);

% 代价函数F
f_list(1:map.X,1:map.Y)=inf;
% 代价函数G
g_list(1:map.X,1:map.Y)=inf;

% 建立全部地图种类，空白区域值为0，障碍物的值为1，open_list为2，close_list为3，未发现的障碍物为4
maps_type=zeros(map.X,map.Y);

% 将未发现的障碍物的值设为4
% for i=1:length(obstacle(:,1))
%     maps_type(obstacle(i,1),obstacle(i,2))=4;
% end
% 一开始跑的时候还不知道路程情况。

% 处理节点，将起始点添加至open_list，添加父节点,代价函数F、G
maps_type(map.start(1), map.start(2))=2;
father_list(map.start(1), map.start(2),:)=[map.start(1), map.start(2)];
g_list(map.start(1), map.start(2))=0;
f_list(map.start(1), map.start(2))=g_list(map.start(1), map.start(2))+HList(map.start(1), map.start(2));
map.CurrentStart=map.start;

% 实际行走路径
RealPath=zeros(1000,2);
RealPathIndex=1;

%更新状态--下一步的八个点
next=MotionModel();

% 是否找到
findFlag=false;
td=0;tab=0;tc=0;tp=0;
run_num=0;
arrive_flag=0;% 是否到达

FoundedList=[]; % 已发现的障碍物

while ~arrive_flag
    while ~findFlag
        run_num=run_num+1;

    %     %--------------------首先判断是否达到目标点，或无路径-----
        % 查找openlist还有没有节点
        if isempty(find(maps_type == 2,1))
            disp('No path to goal!!');
            return;
        end

        % 查看目标是否进入openlist
        if(maps_type(map.goal(1),map.goal(2))==2)
            disp('Find Goal!!');
            maps_type(map.goal(1),map.goal(2))=3;
            paint2(map,maps_type)
            findFlag=true;
            break;
        end

        %------------------a.查找F值最小的节点,并将该点F值置为inf---
        [currentX,currentY]=find(f_list==min(min(f_list)));
        currentX=currentX(1);currentY=currentY(1); % 只要一个最小值
        f_list(currentX,currentY)=inf;

        %------------------b.将F值最小的节点，放到close中，作为当前节点--------------
        maps_type(currentX,currentY)=3;

        %--------------------c.对当前节点周围的8个相邻节点进行计算，算法的主体：------------------------
        for in=1:length(next(:,1))
            if (abs(next(in,1))==1&&abs(next(in,2))==1)
                if (maps_type(currentX+next(in,1),currentY)==1&&maps_type(currentX,currentY+next(in,2))==1)
                    continue;
                end
            end
            % 是否是空白区
            if(maps_type(currentX+next(in,1),currentY+next(in,2))==0||maps_type(currentX+next(in,1),currentY+next(in,2))==4)
                % 是空白区，加入空白区，设置父节点，添加F值
                nextX = currentX+next(in,1);nextY = currentY+next(in,2);

                maps_type(nextX,nextY) = 2;% 加入openlist
                father_list(nextX,nextY,:) = [currentX, currentY];% 添加父节点
                g_list(nextX, nextY)=next(in,3)+g_list(currentX,currentY);% 计算代价G值=当前点到上一点G值+上一点到起点G值
                f_list(nextX,nextY) = HList(nextX,nextY) + g_list(nextX, nextY);% 添加F值,F=H+从当前点到起点距离（G）

            elseif(maps_type(currentX+next(in,1),currentY+next(in,2))==2) % 已经在openlist里面
                nextX = currentX+next(in,1);nextY = currentY+next(in,2);

                new_g=next(in,3)+g_list(currentX,currentY); % 从起点经过当前点到下一点的G值
                if(g_list(nextX,nextY)>new_g)  %下一点的旧g值大于从起点经当前点的g值
                    % 从起点经当前点到下一点的G值更小，重设当前节点父节点和fg值
                    father_list(nextX,nextY,:)=[currentX, currentY];% 重新设置父节点
                    g_list(nextX, nextY)=new_g;% 更新G值
                    f_list(nextX,nextY)=new_g+HList(nextX,nextY);% 更新F值
                end

            end

        end

        if mod(run_num,100)==0
            paint2(map,maps_type)
        end

    end
    % 找到路径，绘制路径
    [closeX,closeY]=find(maps_type == 3);
    fatherX=zeros(length(closeX),1);fatherY=zeros(length(closeX),1);
    for i = 1:length(closeX)
    fatherX(i)=father_list(closeX(i),closeY(i),1);
    fatherY(i)=father_list(closeX(i),closeY(i),2);
    end
    close=[closeX,closeY,fatherX,fatherY];
    path=GetPath2(close,map.CurrentStart,map.goal);


    % 再次初始化地图种类，这导致该算法每次都会重新查找障碍物，但是我懒得改了
    maps_type=zeros(map.X,map.Y);
    % 代价函数F
    f_list(1:map.X,1:map.Y)=inf;
    % 代价函数G
    g_list(1:map.X,1:map.Y)=inf;
    % 将所有发现的障碍物填充
    if ~isempty(FoundedList)
    for FoundObsIndex=1:length(FoundedList(:,1))
        maps_type(FoundedList(FoundObsIndex,1),FoundedList(FoundObsIndex,2))=1; % 发现的障碍物
    end
    end    


% 已经获取路径，开始行动
    for current_place=1:length(path)
        RealPath(RealPathIndex,:)=path(current_place,:);
        RealPathIndex=RealPathIndex+1;
        
        % 到达终点，停止运算
        if path(current_place,1)==map.goal(1)&&path(current_place,2)==map.goal(2)
            RealPath(all(RealPath==0,2),:)=[];
            arrive_flag=1;
            break;
        end
        
        % 探测障碍物
        DetectiveObsIndex=( (obstacle(:,1)-path(current_place,1)).^2+...
            (obstacle(:,2)-path(current_place,2)).^2 ) < detective_area^2;
        DetectiveObsList=DetectiveObsIndex.*obstacle;
        DetectiveObsList(all(DetectiveObsList==0,2),:)=[];
        % 修改maps_type,将障碍物修改为1
        for NewObsIndex=1:length(DetectiveObsList(:,1))
            maps_type(DetectiveObsList(NewObsIndex,1),DetectiveObsList(NewObsIndex,2))=1;
        end
        % 添加到以发现障碍物列表中，去除重复障碍物
        FoundedList=[FoundedList;DetectiveObsList];
        FoundedList=unique(FoundedList,'rows');
        
        % 如果下一个点是障碍物，重新开始计算路径
        if maps_type(path(current_place+1,1),path(current_place+1,2))==1
        maps_type(path(current_place,1),path(current_place,2))=2;
        map.CurrentStart=path(current_place,:);
        father_list(path(current_place,1),path(current_place,2),:)=[path(current_place,1),path(current_place,2)];
        g_list(path(current_place,1),path(current_place,2))=0;
        f_list(path(current_place,1),path(current_place,2))=g_list(path(current_place,1),path(current_place,2))+...
            HList(path(current_place,1),path(current_place,2));
        findFlag=0;
            break;
        end
    end
end
end

function paint(map,maps_type,tp)
    
    %=====绘制======t
    tic
    PlotGrid(map);
    hold on;
    pause(0.01);
    disp(['绘制网格时间：',num2str(toc)])
    %绘制节点close和open节点
    tic
    [x,y]=find(maps_type==3);
    FillPlot([x,y],'r');
    disp(['绘制close-list时间：',num2str(toc)])
    hold on;
    tic
    [x,y]=find(maps_type==2);
    FillPlot([x,y],'g')
    disp(['绘制open-list时间：',num2str(toc)])
    hold on;
    tp=toc+tp;
    disp(['100个点的时间：',num2str(tp)])
    tic
end

function paint2(map,maps_type)
%     tic
    PlotGrid2(map,maps_type,[1 1 1;0 0 0;0 1 0;135/255,206/255,235/255])
%     disp(['time：',num2str(toc)])
end